#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/transforms.h>

/// This file transforms the point cloud from one frame_id to an other frame_id

ros::Publisher point_cloud_pub;
tf::TransformListener *listener;	

void cmdCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_in) {
	sensor_msgs::PointCloud2 cloud_transformed;

	try{
		//ROS_INFO("Transforming...");
		listener->waitForTransform("/fake_scan_optical", (*cloud_in).header.frame_id, (*cloud_in).header.stamp, ros::Duration(10.0) );
		pcl_ros::transformPointCloud ("/fake_scan_optical", *cloud_in, cloud_transformed, *listener);
		point_cloud_pub.publish(cloud_transformed);
	}
	catch(tf::TransformException e){
		ROS_INFO("Transform_PointCloud: %s", e.what());
	}
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "pointcloud_transform");
  ros::NodeHandle n;

	listener = new tf::TransformListener();
  point_cloud_pub = n.advertise<sensor_msgs::PointCloud2>("/cloud_transformed", 10);
  ros::Subscriber point_cloud_sub = n.subscribe("/cloud_throttled", 10, cmdCallback);

	ros::spin();	

	return 0;
}





